#include "axisgrouptest.h"
#include "../axisgroup.h"
#include "../JointDecouple/dataanalysizerdouble.h"
#include "QDebug"
#include "TimeTest/measuretime.h"

AxisGroupTest::AxisGroupTest()
{
}
int    AxisGroupTest::test1()
{

    QVector<JointAttribute> axisGroupAttribute;
    JointAttribute attribute;
    attribute.maxAccelation=12;
    attribute.maxVelocity=50;
    axisGroupAttribute.append(attribute);

    attribute.maxAccelation=22;
    attribute.maxVelocity=70;
    axisGroupAttribute.append(attribute);

    attribute.maxAccelation=8;
    attribute.maxVelocity=60;
    axisGroupAttribute.append(attribute);

    AxisGroup axisGroup(axisGroupAttribute,0,0.1,ENUM_SCARA);
    QVector<double> positionVector;
    positionVector.resize(axisGroupAttribute.size());
    positionVector[0]=100;
    positionVector[1]=200;
    positionVector[2]=-500;

    QVector<double> velVector;
    velVector.resize(axisGroupAttribute.size());
    velVector[0]=2;
    velVector[1]=-18;
    velVector[2]=30;

    QVector<double> accVector;
    accVector.resize(axisGroupAttribute.size());
    accVector[0]=88;
    accVector[1]=-58;
    accVector[2]=-68;


    double speedPercent;
    speedPercent=1;

    QVector<int> axisIdVector;
    axisIdVector .resize(axisGroupAttribute.size());
    axisIdVector[0]=0;
    axisIdVector[1]=1;
    axisIdVector[2]=2;

    //axisGroup.asyncTrapMove(positionVector,speedPercent,axisIdVector);
    //axisGroup.syncTrapMove(positionVector,speedPercent,axisIdVector);
    //axisGroup.syncSplineZeroMove(positionVector,speedPercent,axisIdVector);
    axisGroup.syncSplineContinueMoveByTime(positionVector,velVector,accVector,3,axisIdVector);
    axisGroup.setTimeAxisScale(1,1);

    std::vector<double> axisData;
    axisData.resize(4*axisGroupAttribute.size());
    DataAnalysizerDouble dataAnalysizer("axis data analysizer!!!\n",1,1000,4*axisGroupAttribute.size(),10000,0);
    while(1==axisGroup.moveOneStep())
    {
        for(int i=0;i<axisGroupAttribute.size();i++)
         {
            axisData[i*4+0]=axisGroup.axisArry[i]->positionCommand;
            axisData[i*4+1]=axisGroup.axisArry[i]->velCommand;
            axisData[i*4+2]=axisGroup.axisArry[i]->accCommand;
            axisData[i*4+3]=axisGroup.axisArry[i]->torqueCommand;
        }

        dataAnalysizer.recordDataForAnalysize(axisData);

    }

    dataAnalysizer.writeDataQueueToFile("MultiAxisData.csv");

    return 1;
}


int    AxisGroupTest::test2()
{

    QVector<JointAttribute> axisGroupAttribute;
    JointAttribute attribute;
    attribute.maxAccelation=400;
    attribute.maxVelocity=180;
    attribute.maxTorque=1;
    attribute.maxPosition=180;
    attribute.minPosition=-180;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    attribute.jointType=0;
               attribute.pulsePerRound=100000;
    attribute.externAxisFlag=0;
                  attribute.gearRatio=80;
    attribute.jointName="J1";
    attribute.mobile=true;
    attribute.processFollowError=200;
    attribute.screwPitch=0;
    attribute.targetFollowError=6;
    axisGroupAttribute.append(attribute);

    attribute.maxAccelation=800;
    attribute.maxVelocity=360;
    attribute.maxTorque=1;
    attribute.maxPosition=180;
    attribute.minPosition=-180;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    attribute.jointType=0;
         attribute.pulsePerRound=100000;
    attribute.externAxisFlag=0;
            attribute.gearRatio=50;
    attribute.jointName="J２";
    attribute.mobile=true;
    attribute.processFollowError=200;
    attribute.screwPitch=3;
    attribute.targetFollowError=6;
    axisGroupAttribute.append(attribute);

    attribute.maxAccelation=200;
    attribute.maxVelocity=90;
    attribute.maxTorque=1;
    attribute.maxPosition=90;
    attribute.minPosition=-90;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    attribute.jointType=0;
    attribute.pulsePerRound=100000;
    attribute.externAxisFlag=0;
    attribute.gearRatio=120;
    attribute.jointName="J３";
    attribute.mobile=true;
    attribute.processFollowError=200;
    attribute.screwPitch=3;
    attribute.targetFollowError=6;
    axisGroupAttribute.append(attribute);

    attribute.maxAccelation=400;
    attribute.maxVelocity=50;
    attribute.maxTorque=20;
    attribute.maxPosition=100;
    attribute.minPosition=-100;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    attribute.jointType=0;
    attribute.pulsePerRound=100000;
    attribute.externAxisFlag=0;
    attribute.gearRatio=5;
    attribute.jointName="J４";
    attribute.mobile=true;
    attribute.processFollowError=2000000;
    attribute.screwPitch=3;
    attribute.targetFollowError=6;
    axisGroupAttribute.append(attribute);

    AxisGroup axisGroup(axisGroupAttribute,0,0.1,ENUM_SCARA);
    QVector<double> positionVector;
    positionVector.resize(axisGroupAttribute.size());
    positionVector[0]=50;
    positionVector[1]=-100;
    positionVector[2]=30;
    positionVector[3]=40;

    QVector<double> velVector;
    velVector.resize(axisGroupAttribute.size());
    velVector[0]=2;
    velVector[1]=-18;
    velVector[2]=30;
    velVector[3]=50;
    QVector<double> accVector;
    accVector.resize(axisGroupAttribute.size());
    accVector[0]=8;
    accVector[1]=-5;
    accVector[2]=-6;
    accVector[3]=40;

    QVector<int> axisIdVector;
    axisIdVector .resize(axisGroupAttribute.size());
    axisIdVector[0]=0;
    axisIdVector[1]=1;
    axisIdVector[2]=2;
    axisIdVector[3]=3;

    double speedPercent;
    speedPercent=1;
    axisGroup.setTimeAxisScale(1,3);

    //axisGroup.asyncTrapMove(positionVector,speedPercent,axisIdVector);
    //axisGroup.syncTrapMove(positionVector,speedPercent,axisIdVector);
    //axisGroup.syncSplineZeroMove(positionVector,speedPercent,axisIdVector);
    //axisGroup.syncSplineContinueMove(positionVector,velVector,accVector,axisIdVector);
    axisGroup.syncSplineContinueMoveByTime(positionVector,velVector,accVector,4,axisIdVector);
    MeasureTime mTime("time measure", 1 ,1,2000);
    DataAnalysizerDouble* dataAnalysizerForJointCommand;
    dataAnalysizerForJointCommand=new DataAnalysizerDouble("dataAnalysizerForJointCommand!",
                                                           0, 5000, 4*axisGroupAttribute.size(),4000, 2);
    int isFinish;
    std::vector<double> tmpDataVector;
    tmpDataVector.resize(4*axisGroupAttribute.size());
    do
    {
//        qDebug()<<"time=="<<axisGroup.axisArry[0]->accumulatedTime;
//        qDebug()<<"j1 time duration=="<<axisGroup.axisArry[0]->velProfile->Duration();
//        qDebug()<<"j2 time duration=="<<axisGroup.axisArry[1]->velProfile->Duration();
//        qDebug()<<"j3 time duration=="<<axisGroup.axisArry[2]->velProfile->Duration();
//        qDebug()<<"j4 time duration=="<<axisGroup.axisArry[3]->velProfile->Duration();
        mTime.recordInTime();
        axisGroup.moveOneStep();

        mTime.recordOutTime();

        axisGroup.executeDataToMotor();



        axisGroup.upDateAllFeedbackAndOffset();

        double num=axisGroup.axisArry[0]->velCommand;
        //qDebug()<<"J1 VEL==="<< num ;

        isFinish=axisGroup.isAllVelProfileFinished(axisIdVector);
        //Todo 跟加TargetFollowError判别

        //记录JointCommandDataVector

        for(int i=0;i<axisGroupAttribute.size();i++)
        {
            tmpDataVector[i*4+0]=axisGroup.axisArry[i]->positionCommand ;
            tmpDataVector[i*4+1]=axisGroup.axisArry[i]->velCommand ;
            tmpDataVector[i*4+2]=axisGroup.axisArry[i]->accCommand ;
            tmpDataVector[i*4+3]=axisGroup.axisArry[i]->torqueCommand ;
        }
        dataAnalysizerForJointCommand->recordDataForAnalysize(tmpDataVector);



    }
    while (0==isFinish);

    dataAnalysizerForJointCommand->writeDataQueueToFile("axisGroupJointCommand0.csv");

    axisGroup.axisGroupDecouple->dataAnalysizerForJointCommand->writeDataQueueToFile("axisGroupJointCommand.csv");
    axisGroup.axisGroupDecouple->dataAnalysizerForJointFeedback->writeDataQueueToFile("axisGroupJointFeedback.csv");
    axisGroup.axisGroupDecouple->dataAnalysizerForMotorCommand->writeDataQueueToFile("axisGroupMotorCommand.csv");
    axisGroup.axisGroupDecouple->dataAnalysizerForMotorFeedback->writeDataQueueToFile("axisGroupMotorFeedback.csv");



    return 1;
}
